﻿using System;
using System.Threading;
using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;
using SecretLabs.NETMF.Hardware;
using SecretLabs.NETMF.Hardware.Netduino;
using System.IO.Ports;
using XBee.Library;
using IMU6DOFV2.Netduino;
using XBee.Communications;
using WayPointNavigator;

namespace UAV.OnBoard
{
    public class Program
    {
        #region Members
        internal static int SleepInterval = 100;
        
        internal static Me me;
        internal static XBEE xbee;
        internal static CircularQueue waypoints;
        internal static bool Iniziated = false; 
        #endregion

        #region Main Loop
        public static void Main()
        {
            #region Init Component
            waypoints = new CircularQueue();
            me = new Me(new IMU6DOFv2(Serial.COM1));
            xbee = new XBEE(Serial.COM2, new IMUData());
            xbee.OnReciveData += new ReciveDataDelegate(xbee_OnReciveData);
            #endregion

            #region Board Inicialization
            while (!Iniziated && !me.Data.IsGPSOnSignal)
            {
                Thread.Sleep(SleepInterval);
                //Read the source file
                ReadRouteInfo();
                Iniziated = true;
            }
            // Set Destination To Nearest
            waypoints.CycleTo(waypoints.GetClosestToMe(me.Location));
            #endregion

            #region Main Loop
            while (true)
            {
                Thread.Sleep(SleepInterval);

                // Set our destination point that we'll be using throughout this method.
                GpsPoint dest = waypoints.Peek() as GpsPoint;

                // Are we close to the current waypoint? If so, push it to the end and move to the next.
                if (dest.Distance(me.Location) < 3)
                {
                    waypoints.Dequeue();
                    continue;
                }

                if (!me.IsFacing(dest))
                    me.Face(dest);

                if (!me.IsMoving)
                    me.MoveForward();

                xbee.Write(me.Data);
            }
            #endregion
        }
        #endregion

        #region Private Methods
        static void ReadRouteInfo()
        {
            throw new NotImplementedException();
        }
        #endregion        

        #region Internal Events
		static void xbee_OnReciveData(ICommunicationData data)
        {
            Debug.Print("Read Data!"+ data.Serialize());
        }
	    #endregion    
    }
}
